package com.kopo.droidioiolib;

import java.io.IOException;
import java.io.InputStream;
import java.io.OutputStream;
import java.util.ArrayList;
import java.util.List;

import android.util.Log;
import ioio.lib.api.AnalogInput;
import ioio.lib.api.DigitalInput;
import ioio.lib.api.DigitalOutput;
import ioio.lib.api.DigitalOutput.Spec.Mode;
import ioio.lib.api.Uart;
import ioio.lib.api.Uart.Parity;
import ioio.lib.api.Uart.StopBits;
import ioio.lib.api.exception.ConnectionLostException;

public class LipoKitInterface extends ControlInterface {
	private static final String TAG = "kopo.log";
	public List<DigitalOutput> pwmDir2Outputs = new ArrayList<DigitalOutput>();
	private int[] pwmDuties = new int[2];
	private DigitalOutput stby = null;
	private DigitalInput ir1 = null;
	private DigitalInput ir2 = null;
	private AnalogInput ir3 = null;
	private Uart uart = null;
	private CameraCmd cameraCommandQue = CameraCmd.NONE;	// 

	public enum CameraCmd {
	    NONE, SYNC 
	}

    @Override
    public void setup() {
    	Log.d(TAG, "IOIOSingle.setup()");

    	// Setup IOIO Pins. Initialize PWM and servo pins
    	isConnected = true;

		try {

	    	//----- ADD PWMs
	    	stby = ioio_.openDigitalOutput(37);  // STBY
	    	stby.write(true);		// stby mode

	    	//pwm1 = ioio_.openPwmOutput(10, 100);   //  in the normal (i.e. not open-drain) mode, 100Hz = 10ms
	    	pwmOutputs.add( ioio_.openPwmOutput(new DigitalOutput.Spec(34, Mode.NORMAL), 100) );   //  in the normal (i.e. not open-drain) mode, 100Hz = 10ms
	    	pwmDir2Outputs.add( ioio_.openDigitalOutput(35) );  // AIN2		CCW when H
	    	pwmDirOutputs.add( ioio_.openDigitalOutput(36) );  // AIN1		CW when H
	    	pwmOutputs.get(0).setDutyCycle(0);
	    	pwmDirOutputs.get(0).write(false);
	    	pwmDir2Outputs.get(0).write(false);

	    	//pwm2 = ioio_.openPwmOutput(12, 100);   // 10ms, range 0-255
	    	pwmOutputs.add( ioio_.openPwmOutput(new DigitalOutput.Spec(40, Mode.NORMAL), 100));   //  in the normal (i.e. not open-drain) mode, 100Hz = 10ms
	    	pwmDirOutputs.add( ioio_.openDigitalOutput(38) );  // BIN1   CW when H
	    	pwmDir2Outputs.add( ioio_.openDigitalOutput(39) );  // BIN2  CCW when H
	    	pwmOutputs.get(1).setDutyCycle(0);
	    	pwmDirOutputs.get(1).write(false);
	    	pwmDir2Outputs.get(1).write(false);
	    	pwmDuties[0] = 0;
	    	pwmDuties[1] = 0;

	    	ir1 = ioio_.openDigitalInput(22, DigitalInput.Spec.Mode.PULL_DOWN);		// low when detect object
	    	ir2 = ioio_.openDigitalInput(23, DigitalInput.Spec.Mode.PULL_DOWN);		// low when detect object
	    	
	    	ir3 = ioio_.openAnalogInput(46);
	    	
	    	//--- ADD digital inputs  
	    	led = ioio_.openDigitalOutput(0, false);

	    	//--- Add UART for Camera rx = 6, tx = 7
	    	//uart = ioio_.openUart(6, 7, 115200, Parity.NONE, StopBits.ONE);

		} catch (ConnectionLostException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}    	
    }

    // dutyCycle in percent 0-100
    // dir = 0 : cw
    // dir = 1 : ccw
    // dir = 2 : stop
    @Override
    public void setPwm(int pwmNo, int dutyCycle, int dir)
    {
    	if( !isConnected ) return;
    	if( pwmNo >= pwmOutputs.size() ) return;

    	pwmDuties[pwmNo] = dutyCycle;
    	
		try {
			pwmOutputs.get(pwmNo).setDutyCycle( ((float)dutyCycle)/100.0f);
			switch( dir )
			{
			case 0: // cw
				pwmDirOutputs.get(pwmNo).write(true);
				pwmDir2Outputs.get(pwmNo).write(false);
				break;
			case 1: // ccw
				pwmDirOutputs.get(pwmNo).write(false);
				pwmDir2Outputs.get(pwmNo).write(true);
				break;
			case 2: // short brake
				pwmDirOutputs.get(pwmNo).write(true);
				pwmDir2Outputs.get(pwmNo).write(true);
			default: // stop
				pwmDirOutputs.get(pwmNo).write(false);
				pwmDir2Outputs.get(pwmNo).write(false);
			}

			if( pwmDuties[0] == 0 && pwmDuties[1] == 0 )
			{
				stby.write(false);
			}
			else
				stby.write(true);
			//Log.d("kopo.log", "ControlInterface.setPwm: dir=" + dir);
		} catch (ConnectionLostException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
    }

    public float readIR1()
    {
    	boolean r;
		try {
			r = ir1.read();
	    	//Log.d(TAG, "readIR1(): r=" + r);
	    	if( r )
	    		return 1.0f;
	    	else 
	    		return 0;
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		} catch (ConnectionLostException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		return 0;
    }

    public float readIR2()
    {
    	boolean r;
		try {
			r = ir2.read();
	    	//Log.d(TAG, "readIR2(): r=" + r);
	    	if( r )
	    		return 1.0f;
	    	else
	    		return 0;
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		} catch (ConnectionLostException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		return 0;
    }
    
    
    public float readIR3(){
    	
    	float r;
		try {
			r = ir3.getVoltage();
	    	//Log.d(TAG, "readIR1(): r=" + r);
	    	return r;
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		} catch (ConnectionLostException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		return 0;
    }
    
    
    
    private void sendCameraCmd(byte[] cmd)
    {
    	try {
        	OutputStream out = uart.getOutputStream();
			out.write(cmd);
	    	out.close();
		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
    }

    private byte[] readCamera(int offset, int length)
    {
    	// send UART command
    	try {
        	InputStream in = uart.getInputStream();
			byte[] rbuffer = new byte[length];
			in.read(rbuffer, offset, length);
			in.close();
			return rbuffer;
		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		return null;
    }

    private void cameraSync()
    {
    	byte[] ackBuffer = { (byte)0xaa, 0x0d, 0x00, 0x00, 0x00, 0x00};
    	boolean ack = false;
    	do
    	{
        	sendCameraCmd(ackBuffer);
        	byte[] rBuffer = readCamera(0, 6);
        	if( rBuffer != null)
        	{
        		if( rBuffer[0] == (byte) 0xaa && rBuffer[1] == (byte) 0x0e && rBuffer[2] == (byte) 0x0d )
        		{
        			ack = true;
        		}
        	}
    	}
    	while( !ack );
    	cameraCommandQue = CameraCmd.SYNC;

    	Log.d(TAG, "CameraSync completed");
    }

    public void takePicture()
    {
    	// send UART command
    	try {
        	InputStream in = uart.getInputStream();
        	OutputStream out = uart.getOutputStream();
        	byte[] buffer = { (byte)0xaa, 0x04, 0x00, 0x02, 0x01, 0x01};
			out.write(buffer);
			byte[] rbuffer = new byte[4800];
			int offset = 0;
			int length = 4800;
			in.read(rbuffer, offset, length);
		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
    	uart.close();
    }

    public void processCameraCommand()
    {
    	switch(cameraCommandQue )
    	{
    		case SYNC:
            	byte[] rbuffer = readCamera(0, 6);
            	if( rbuffer != null)
            	{
            		if( rbuffer[0] == (byte) 0xaa && rbuffer[1] == (byte) 0x0e && rbuffer[2] == (byte) 0x0d )
            		{
            			byte[] buffer = { (byte)0xaa, 0x0e, 0x0d, 0x00, 0x00, 0x00};
            			sendCameraCmd(buffer);
            			cameraCommandQue = CameraCmd.NONE;
            		}
            	}
    			break;
    	}
    }


}
